import numpy as np
import copy

class vector2d:
    def __init__(self,x=0,y=0):
        self.x=x
        self.y=y
        self.state=0

    def __add__(self, rhs):
        return vector2d(self.x + rhs.x, self.y + rhs.y)

    def __sub__(self, rhs):
        return vector2d(self.x - rhs.x, self.y - rhs.y)

    def __mul__(self, scalar):
        return vector2d(self.x * scalar, self.y * scalar)

    def __div__(self, scalar):
        return vector2d(self.x / scalar, self.y / scalar)

    def dot(self,v):
        return self.x*v.x+self.y*v.y

    def unit(self,step=0.25):
        return vector2d(self.x,self.y)*2*step/(max(np.linalg.norm([self.x,self.y]),0.001))

    def norm(self):
        return np.linalg.norm([self.x,self.y])

    def abs(self):
        return abs(self.x)+abs(self.y)

    @classmethod
    def rotate(cls,vec,direct=1):
        if direct==1:
            vec2=vector2d(-vec.y,vec.x)
        else:
            vec2 = vector2d(vec.y, -vec.x)
        return vec2

    @classmethod
    def getneighbors(cls,pnt,res=0.25):
        neighs = []
        neighs.append(pnt + vector2d(res, res))
        neighs.append(pnt + vector2d(res, -res))
        neighs.append(pnt - vector2d(res, res))
        neighs.append(pnt - vector2d(res, -res))
        return neighs

class stcvertex:
    def __init__(self,x=0,y=0,res=0.25):
        self.center = vector2d(x,y)
        self.res = res
        self.edges = {}
        self.state = "unreached"
        self.overreached=False

    def edgeinfo(self):
        str=""
        for e in self.edges:
            str = str+e+"(%.2f,%.2f)"%(self.edges[e].center.x,self.edges[e].center.y)
        return str

    def step(self,agentpos, direction=1):
        vec=agentpos-self.center
        vec2=vector2d.rotate(vec,direction)
        vn=vec2-vec

        for e in self.edges:
            edge = self.edges[e]
            v = edge.center-self.center
            #print [self.center.x,self.center.y],[edge.center.x,edge.center.y],v.dot(vn),v.dot(vec+vec2)

            if v.dot(vn) == 0 and v.dot(vec+vec2) > 0:
                vn = v.unit()
                break
        newpos = agentpos+vn
        newvertex = self.snipcenter(newpos)
        #print "Now I am at (%.2f,%.2f), centered at (%.2f,%.2f), whose edges are "\
        #      %(newpos.x,newpos.y,newvertex.center.x,newvertex.center.y)\
        #      +newvertex.edgeinfo()
        return newpos,newvertex

    def snipcenter(self,agent):
        neighs = vector2d.getneighbors(agent)
        for n in neighs:
            k = str(n.x*1000+n.y)
            if self.edges.has_key(k):
                return self.edges[k]
        return self


class stcgraph:
    def __init__(self,vec):
        self.vertexes = {}
        self.graphvertexes = {}
        self.ocvertexes={}
        for v in vec:
            self.vertexes[str(v.x*1000+v.y)] = stcvertex(v.x,v.y)

    def gentrj(self,agentpos,vertex):
        ipos = copy.deepcopy(agentpos)
        trj = []
        trj.append([agentpos.x, agentpos.y])
        i=0
        while(abs(ipos.x-agentpos.x)+abs(ipos.y-agentpos.y)>0 or len(trj)<3):
            agentpos,vertex=vertex.step(agentpos)
            trj.append([agentpos.x,agentpos.y])
            i=i+1
            if(i>1000):
                break
        return trj

    def graphcenter(self):
        pos=vector2d()
        for v in self.vertexes:
            vv=self.vertexes[v]
            pos=pos+vv.center
        pos=pos/len(self.vertexes)
        return vector2d(x=round(pos.x),y=round(pos.y))

    def isfullycovered(self):
        for v in self.vertexes:
            if not (self.vertexes[v].state=="reached"):
                return False
        return True

    def getneighbors(self, vert, res=1):
        neighs = []
        vv=vert
        n= vv + vector2d(0, res)
        k= str(n.x*1000+n.y)
        if self.vertexes.has_key(k):
            neighs.append(self.vertexes[k])

        n = vv + vector2d(res, 0)
        k = str(n.x * 1000 + n.y)
        if self.vertexes.has_key(k):
            neighs.append(self.vertexes[k])

        n = vv - vector2d(0, res)
        k = str(n.x * 1000 + n.y)
        if self.vertexes.has_key(k):
            neighs.append(self.vertexes[k])

        n = vv - vector2d(res, 0)
        k = str(n.x * 1000 + n.y)
        if self.vertexes.has_key(k):
            neighs.append(self.vertexes[k])
        return neighs

    def span(self, agentpos, res=1):
        neighs=vector2d.getneighbors(agentpos)
        #print "The start point is:%f,%f"%(agentpos.x,agentpos.y)
        startpoint=None
        for n in neighs:
            k=str(n.x*1000+n.y)
            if self.vertexes.has_key(k):
                startpoint=self.vertexes[k]
        if startpoint==None:
            print "STC Graph - span: I cannot find a vertex to start!"
            return
        self.graphvertexes[str(startpoint.center.x*1000+startpoint.center.y)]=copy.deepcopy(startpoint)
        gpc=0
        while(len(self.graphvertexes)<len(self.vertexes)):
            flag=False
            kg = ''
            kv = ''
            for v in self.graphvertexes:
                vv=self.graphvertexes[v].center
                neighs = []
                neighs.append(vv + vector2d(0, res))
                neighs.append(vv + vector2d(res, 0))
                neighs.append(vv - vector2d(0, res))
                neighs.append(vv - vector2d(res, 0))
                for n in neighs:
                    k=str(n.x*1000+n.y)
                    if self.vertexes.has_key(k) and (not self.graphvertexes.has_key(k)):
                        kg=k
                        kv=v
                        flag=True
                        break
                if flag:
                    break
            if flag:
                self.graphvertexes[kg]=copy.deepcopy(self.vertexes[kg])
                self.graphvertexes[kv].edges[kg]=self.graphvertexes[kg]
                self.graphvertexes[kg].edges[kv] = self.graphvertexes[kv]
                # #print "Now connect (%.1f,%.1f) with (%.1f,%.1f)" % (self.graphvertexes[kv].center.x,\
                #                                                     self.graphvertexes[kv].center.y,\
                #                                                     self.graphvertexes[kg].center.x,\
                #                                                     self.graphvertexes[kg].center.y)+kg+" "+kv
            if(len(self.graphvertexes)==gpc):
                break
            gpc=len(self.graphvertexes)


